/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * File Name          : app_freertos.c
  * Description        : Code for freertos applications
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2024 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
/* USER CODE END Header */

/* Includes ------------------------------------------------------------------*/
#include "FreeRTOS.h"
#include "task.h"
#include "main.h"
#include "cmsis_os.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include <string.h>
#include "foc.h"
#include "pwm_curr.h"
#include "encoder.h"
#include "controller.h"
#include "mc_task.h"
#include "anticogging.h"
#include "usr_config.h"
#include "usart.h"
#include "lwrb.h"
#include "serial.h"

/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

#define LWRB_BUFF_SZIE  4096
lwrb_t g_lwrb;
uint8_t s_lwrbBuf[LWRB_BUFF_SZIE];

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */

/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

#define CH_COUNT 10
struct Frame
{
    float fdata[CH_COUNT];
    unsigned char tail[4];
};

struct Frame vfoaFrame = {
        .tail = {0x00, 0x00, 0x80, 0x7f}
};

uint8_t vofaCmdBuf[1024];


/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN Variables */

volatile uint32_t SystickCount = 0;


osThreadId_t gSerialDataParseTaskHandle;
const osThreadAttr_t gSerialDataParseTask_attributes = {
        .name = "gSerialDataParseTask",
        .priority = (osPriority_t) osPriorityNormal,
        .stack_size = 512 * 4
};

/* USER CODE END Variables */
/* Definitions for defaultTask */
osThreadId_t defaultTaskHandle;
const osThreadAttr_t defaultTask_attributes = {
        .name = "defaultTask",
        .priority = (osPriority_t) osPriorityNormal,
        .stack_size = 512 * 4
};
/* Definitions for gBtnScanTask */
osThreadId_t gBtnScanTaskHandle;
const osThreadAttr_t gBtnScanTask_attributes = {
        .name = "gBtnScanTask",
        .priority = (osPriority_t) osPriorityNormal,
        .stack_size = 512 * 4
};

/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN FunctionPrototypes */

void StartSerialDataParseTask(void *argument);

/* USER CODE END FunctionPrototypes */

void StartDefaultTask(void *argument);

void StartBtnScanTask(void *argument);

void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */

/**
  * @brief  FreeRTOS initialization
  * @param  None
  * @retval None
  */
void MX_FREERTOS_Init(void)
{
    /* USER CODE BEGIN Init */

    /* 创建用于串口通讯的ringbuffer */
    lwrb_init(&g_lwrb, s_lwrbBuf, LWRB_BUFF_SZIE);

    /* vofa+ debug */
    HAL_UARTEx_ReceiveToIdle_DMA(&huart3, vofaCmdBuf, 1024);


    if (USR_CONFIG_read_config() == 0)
    {
        printf("Config loaded\n");
    }
    else
    {
        USR_CONFIG_set_default_config();
        printf("Config loaded faile set to default\n");
    }
    HAL_Delay(500);

    if (USR_CONFIG_read_cogging_map() == 0)
    {
        AnticoggingValid = true;
        printf("Cogging map loaded\n");
    }
    else
    {
        USR_CONFIG_set_default_cogging_map();
        printf("Cogging map loaded faile set to default\n");
    }
    HAL_Delay(500);


    MCT_init();
    FOC_init();
    PWMC_init();
    ENCODER_init();
    CONTROLLER_init();

    // MCT_safety_task timer
    HAL_TIM_Base_Start_IT(&htim6);

    HAL_Delay(500);

    MCT_set_state(IDLE);

    /* USER CODE END Init */

    /* USER CODE BEGIN RTOS_MUTEX */
    /* add mutexes, ... */
    /* USER CODE END RTOS_MUTEX */

    /* USER CODE BEGIN RTOS_SEMAPHORES */
    /* add semaphores, ... */
    /* USER CODE END RTOS_SEMAPHORES */

    /* USER CODE BEGIN RTOS_TIMERS */
    /* start timers, add new ones, ... */
    /* USER CODE END RTOS_TIMERS */

    /* USER CODE BEGIN RTOS_QUEUES */
    /* add queues, ... */
    /* USER CODE END RTOS_QUEUES */

    /* Create the thread(s) */
    /* creation of defaultTask */
    defaultTaskHandle = osThreadNew(StartDefaultTask, NULL, &defaultTask_attributes);

    /* creation of gBtnScanTask */
    gBtnScanTaskHandle = osThreadNew(StartBtnScanTask, NULL, &gBtnScanTask_attributes);

    /* USER CODE BEGIN RTOS_THREADS */
    /* add threads, ... */

    /* creation of gSerialDataParseTask */
    gSerialDataParseTaskHandle = osThreadNew(StartSerialDataParseTask, NULL, &gSerialDataParseTask_attributes);

    /* USER CODE END RTOS_THREADS */

    /* USER CODE BEGIN RTOS_EVENTS */
    /* add events, ... */


    /* USER CODE END RTOS_EVENTS */

}

/* USER CODE BEGIN Header_StartDefaultTask */
/**
  * @brief  Function implementing the defaultTask thread.
  * @param  argument: Not used
  * @retval None
  */
/* USER CODE END Header_StartDefaultTask */
void StartDefaultTask(void *argument)
{
    /* USER CODE BEGIN StartDefaultTask */
    /* Infinite loop */
    for (;;)
    {
        MCT_low_priority_task();

        osDelay(1);
    }
    /* USER CODE END StartDefaultTask */
}

/* USER CODE BEGIN Header_StartBtnScanTask */
/**
* @brief Function implementing the gBtnScanTask thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_StartBtnScanTask */
void StartBtnScanTask(void *argument)
{
    /* USER CODE BEGIN StartBtnScanTask */
    /* Infinite loop */
    for (;;)
    {
//        MCT_safety_task();
//        SystickCount++;

        osDelay(1);
    }

    /* USER CODE END StartBtnScanTask */
}

/* Private application code --------------------------------------------------*/
/* USER CODE BEGIN Application */


/**
 * @brief
 * @param argument
 */
void StartSerialDataParseTask(void *argument)
{
    uint8_t pack[32] = {0};
    uint8_t packHead = 0xAA;
    size_t findIdx = 0;

    for (;;)
    {
        if (lwrb_get_full(&g_lwrb) >= sizeof(SerialFrame_t))
        {
            lwrb_find(&g_lwrb, &packHead, 1, 0, &findIdx);
            lwrb_skip(&g_lwrb, findIdx);
            lwrb_read(&g_lwrb, pack, sizeof(SerialFrame_t));

            CAN_receive_callback(pack, sizeof(SerialFrame_t));
        }

        osDelay(1);
    }
}


/**
  * @brief  Period elapsed callback in non blocking mode
  * @note   This function is called  when TIM20 interrupt took place, inside
  * HAL_TIM_IRQHandler(). It makes a direct call to HAL_IncTick() to increment
  * a global variable "uwTick" used as application time base.
  * @param  htim : TIM handle
  * @retval None
  */
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
    /* USER CODE BEGIN Callback 0 */

    /* USER CODE END Callback 0 */
    if (htim->Instance == TIM20) {
        HAL_IncTick();
    }
    /* USER CODE BEGIN Callback 1 */

    if (htim->Instance == TIM6) {
        MCT_safety_task();
        SystickCount++;
    }

    /* USER CODE END Callback 1 */
}


/**
  * @brief  Injected conversion complete callback in non-blocking mode.
  * @param hadc ADC handle
  * @retval None
  */
void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *hadc)
{
    /* Calc ADC offset */
    static int adc_sum_a = 0;
    static int adc_sum_b = 0;
    static int adc_sum_c = 0;
    static bool isCalcAdcOffsetOvered = false;
    const int measCnt = 64;
    static int measCntCopy = measCnt;
    if (hadc->Instance == ADC1 && !isCalcAdcOffsetOvered)
    {
        adc_sum_a += HAL_ADCEx_InjectedGetValue(&hadc1, ADC_INJECTED_RANK_1);
        adc_sum_b += HAL_ADCEx_InjectedGetValue(&hadc1, ADC_INJECTED_RANK_2);
        adc_sum_c += HAL_ADCEx_InjectedGetValue(&hadc1, ADC_INJECTED_RANK_3);

        if (--measCntCopy <= 0)
        {
            phase_a_adc_offset = adc_sum_a / measCnt;
            phase_b_adc_offset = adc_sum_b / measCnt;
            phase_c_adc_offset = adc_sum_c / measCnt;

            isCalcAdcOffsetOvered = true;
        }
    }

    if (hadc->Instance == ADC1 && isCalcAdcOffsetOvered)
    {
        MCT_high_frequency_task();

        vfoaFrame.fdata[0] = Foc.i_a;
        vfoaFrame.fdata[1] = Foc.i_b;
        vfoaFrame.fdata[2] = Foc.v_bus_filt;
        vfoaFrame.fdata[3] = Foc.i_bus_filt;
        vfoaFrame.fdata[4] = Foc.i_q_filt;
        vfoaFrame.fdata[5] = Foc.dtc_a;
        vfoaFrame.fdata[6] = Encoder.vel;
        vfoaFrame.fdata[7] = UsrConfig.encoder_dir == 1 ? 1.0f : -1.0f;
        vfoaFrame.fdata[8] = (float) Foc.isSvmOutputValid;

        if (StatuswordNew.errors.over_current == 1)
            vfoaFrame.fdata[9] = 1;
        else if (StatuswordNew.errors.over_voltage == 1)
            vfoaFrame.fdata[9] = 2;
        else if (StatuswordNew.errors.under_voltage == 1)
            vfoaFrame.fdata[9] = 3;
        else if (StatuswordNew.errors.adc_selftest_fatal == 1)
            vfoaFrame.fdata[9] = 4;
        else if (StatuswordNew.errors.encoder_offline == 1)
            vfoaFrame.fdata[9] = 5;
        else if (StatuswordNew.errors.PADDING_1 == 1)
            vfoaFrame.fdata[9] = 6;
        else if (StatuswordNew.errors.PADDING_2 == 1)
            vfoaFrame.fdata[9] = 7;
        else
            vfoaFrame.fdata[9] = 0;

        //HAL_UART_Transmit_DMA(&huart3, (uint8_t *) (&vfoaFrame), sizeof(vfoaFrame));
    }
}


/**
 * @brief Parse vofa cmd
 * @param cmdBuf
 * @return
 */
static float vofa_cmd_parse(uint8_t *cmdBuf, char *arg)
{
    return atof(cmdBuf + strlen(arg));
}

/**
  * @brief  Reception Event Callback (Rx event notification called after use of advanced reception service).
  * @param  huart UART handle
  * @param  Size  Number of data available in application reception buffer (indicates a position in
  *               reception buffer until which, data are available)
  * @retval None
  */
void HAL_UARTEx_RxEventCallback(UART_HandleTypeDef *huart, uint16_t Size)
{
    if (huart->Instance == USART3)
    {
        if (strstr(vofaCmdBuf, "calib"))
        {
            MCT_set_state(CALIBRATION);
        }
        else if (strstr(vofaCmdBuf, "run="))
        {
            float val = vofa_cmd_parse(vofaCmdBuf, "run=");
            if (val > 0)
                MCT_set_state(RUN);
            else
                MCT_set_state(IDLE);
        }
        else if (strstr(vofaCmdBuf, "anticog"))
        {
            MCT_set_state(ANTICOGGING);
        }
        else if (strstr(vofaCmdBuf, "rsetErr"))
        {
            MCT_reset_error();
        }
        else if (strstr(vofaCmdBuf, "torque"))
        {
            float val = vofa_cmd_parse(vofaCmdBuf, "torque=");
            Controller.input_torque_buffer = val;
            CONTROLLER_sync_callback();
        }
        else if (strstr(vofaCmdBuf, "vel"))
        {
            float val = vofa_cmd_parse(vofaCmdBuf, "vel=");
            Controller.input_velocity_buffer = val;
            CONTROLLER_sync_callback();
        }
        else if (strstr(vofaCmdBuf, "pos"))
        {
            float val = vofa_cmd_parse(vofaCmdBuf, "pos=");
            Controller.input_position_buffer = val;
            CONTROLLER_sync_callback();
        }
        else
        {}

        HAL_UARTEx_ReceiveToIdle_DMA(&huart3, vofaCmdBuf, 1024);
    }
}

/* USER CODE END Application */

